#include "nucCommu.h"
#include "CAN_receive.h"
#include "pid.h"
#include "cmsis_os.h"
#include "robot_total_mode.h"
#include "arm_math.h"
#include "OLED.h"
#include "user_lib.h"
#include "detect_task.h"
#include "referee.h"
#include "remote_control.h"

//********************建系方案**************************//
//前进方向为x正方向，竖直向上为w（角速度）正方向，Y=W×X(正方向)
//电机左前等指面向x正方向的左前【顺时针一圈】
//GM6020的中值指轮向前且M3508在内测
//以下规定只在解算时起作用，发送指令时根据实际情况来取符号
//M3508的转动方向取正指GM6020在中值时向前运动的转动方向
//GM6020的转动方向取正为竖直向上【角度取值范围[-90,90]】【和实际相同】
//*************************按键定义********************//
//按键定义
#define HANDLE_LEFT_LR 2    //遥控器左边摇杆的左右方向  left right
#define HANDLE_LEFT_BF 3    //遥控器左边摇杆的前后方向  back forth
#define HANDLE_RIGHT_LR 0    //遥控器右边摇杆的左右方向  left right
#define HANDLE_RIGHT_BF 1    //遥控器右边摇杆的前后方向  back forth

#define COMMON_BF_RC            HANDLE_LEFT_BF      //前进,后退
#define COMMON_LR_ROTATE_RC     HANDLE_LEFT_LR      //左转,右转

#define SPINNER_BF_RC            HANDLE_LEFT_BF      //前进,后退
#define SPINNER_LR_MOVE_RC       HANDLE_LEFT_LR      //左转,右转

//****************************机器人控制常量***************//
#define RC_CH_MAX 873.0f //遥控器通道最大值
#define INT8_MAX 127.0f
#define MAX_SPEED 1000.0f //最大速度
#define MAX_OMEGA 2.0f    //最大角速度
#define CHASSIS_RC_DEADLINE 10  //低于这个值的摇杆读数一律为0
#define CHASSIS_VX_RC_SEN (-MAX_SPEED/RC_CH_MAX)   //把遥控器摇杆读数转化为速度的系数
#define CHASSIS_VY_RC_SEN (MAX_SPEED/RC_CH_MAX)   //是负数，遥控器向左拉，向左平移，遥控器摇杆读数为负，在坐标系中为向y正方向
#define CHASSIS_WZ_RC_SEN (MAX_OMEGA/RC_CH_MAX)   //最大旋转线速度。0.33m/s  //是负数。遥控器右拉，向右转
#define CHASSIS_SPINNER_W 3.0f
#define SWITCH_SA 5       //遥控器两档乒乓开关 SA


//*************************任务相关常量*******************//

#define CHASSIS_TASK_INIT_TIME 500      //电机初始化时间
#define CHASSIS_CONTROL_TIME_MS 5       //每5ms执行一次任务

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

//******************数字滤波所需常量****************// 系数和控制方法来自官方步兵
#define CHASSIS_ACCEL_X_NUM 150.1666f
#define CHASSIS_ACCEL_Y_NUM 150.1666f
#define CHASSIS_ACCEL_W_NUM 150.66f



//限幅函数
#define range_limit_inside(toBeLimited, range)                      \
    {                                                               \
         if((toBeLimited)>(range))                                  \
        {                                                           \
            (toBeLimited)=(range);                                  \
        }                                                           \
        else if((toBeLimited)<-(range))                             \
        {                                                           \
            (toBeLimited)=-(range);                                 \
        }                                                           \
    }
//**************************控制量结构体*******************//


//机器人控制量
struct RobotControl_s{
    fp32 vx,vy,w;
    first_order_filter_type_t vx_filter,vy_filter,w_filter;
};

//云台yaw电机相对于底盘角度结构体
struct Angle_s{
    int16_t nowGimbalYawECD;        //目前的YAW轴云台ECD
    fp32    gimbalAngleFromChassis; //云台相对于最初的角度，逆时针为正方向。单位弧度。
    int16_t nowChessAGVECD[4];        //目前的四个舵轮GM6030ECD
};

//************************用于控制的全局变量**********//
static int zeroCurrentMark;    //当离线时，直接发送零电流的时候
static struct Angle_s someAngle;   //用于记录所有的云台、底盘角度相关值
static toDownSTM_t *toDownlSTM_p;   //遥控器位置指针，需要初始化
static const RC_ctrl_t *rc_p;   //遥控器位置指针，需要初始化
static const enum RobotState_e *robotMode;   //机器人模式
static struct RobotControl_s robotTotalSpeedControl;
static int16_t vx_channel,vy_channel,w_channel; //遥控器deadband limit后输出的值

static void setChassisMode(void)
{
    zeroCurrentMark=0;
    if(RobotState_e_Powerless== *robotMode)
        zeroCurrentMark=1;
    else if(RobotState_e_Brilliant == *robotMode)
    {
        if(game_not_started())
            zeroCurrentMark = 1;
    }
}

static void analyseTotalControl(void)    //通过遥控器和键盘参数设置速度。当键盘输出量为0时，通过遥控器摇杆确定速度。
{
    robotTotalSpeedControl.vx=0;robotTotalSpeedControl.vy=0;robotTotalSpeedControl.w=0;
    
    if(RobotState_e_Powerless==*robotMode)
    {
        return;
    }
    else if(RobotState_e_CommonCar==*robotMode)
    {   
        if(switch_is_up(rc_p->rc.ch[SWITCH_SA])){
            rc_deadband_limit(rc_p->rc.ch[COMMON_LR_ROTATE_RC],vy_channel,CHASSIS_RC_DEADLINE);
            robotTotalSpeedControl.vy +=vy_channel*CHASSIS_VY_RC_SEN;
        }else{
            rc_deadband_limit(rc_p->rc.ch[COMMON_LR_ROTATE_RC],w_channel,CHASSIS_RC_DEADLINE);
            robotTotalSpeedControl.w +=w_channel*CHASSIS_WZ_RC_SEN;
        }
        rc_deadband_limit(rc_p->rc.ch[COMMON_BF_RC],vx_channel,CHASSIS_RC_DEADLINE);
        robotTotalSpeedControl.vx += vx_channel*CHASSIS_VX_RC_SEN ;
    }
    else if(RobotState_e_Spinner==*robotMode){
        rc_deadband_limit(rc_p->rc.ch[COMMON_BF_RC],vx_channel,CHASSIS_RC_DEADLINE);
        rc_deadband_limit(rc_p->rc.ch[SPINNER_LR_MOVE_RC],vy_channel,CHASSIS_RC_DEADLINE);
        robotTotalSpeedControl.vx += vx_channel*CHASSIS_VX_RC_SEN;
        robotTotalSpeedControl.vy +=vy_channel*CHASSIS_VY_RC_SEN;
        robotTotalSpeedControl.w = MAX_OMEGA;
    }else if(RobotState_e_Brilliant==*robotMode){
        
    }
}

void initFilters(void)
{
    const static fp32 chassis_x_order_filter[1] = {CHASSIS_ACCEL_X_NUM};
    const static fp32 chassis_y_order_filter[1] = {CHASSIS_ACCEL_Y_NUM};
    const static fp32 chassis_w_order_filter[1] = {CHASSIS_ACCEL_W_NUM};
    
    first_order_filter_init(&(robotTotalSpeedControl.vx_filter), CHASSIS_CONTROL_TIME_MS, chassis_x_order_filter);
    first_order_filter_init(&(robotTotalSpeedControl.vy_filter), CHASSIS_CONTROL_TIME_MS, chassis_y_order_filter);
    first_order_filter_init(&(robotTotalSpeedControl.w_filter), CHASSIS_CONTROL_TIME_MS, chassis_w_order_filter);

}


void chassis_task(void const *pvParameters)
{
    osDelay(CHASSIS_TASK_INIT_TIME);    //等待电机初始化
    initFilters();
    toDownlSTM_p= get_to_down_stm_point();    //获取遥控器数据和NUC数据指针
    robotMode=getRobotPresentMode();
    rc_p=get_remote_control_point();

    while(1)
    {
        setChassisMode();
        analyseTotalControl(); 
        toDownlSTM_p->x=(int8_t)(robotTotalSpeedControl.vx/MAX_SPEED*INT8_MAX);   
        toDownlSTM_p->y=(int8_t)(robotTotalSpeedControl.vy/MAX_SPEED*INT8_MAX);  
        toDownlSTM_p->w=(int8_t)(robotTotalSpeedControl.w/MAX_OMEGA*INT8_MAX);  
        toDownlSTM_p->nextmode=*robotMode;
        osDelay(CHASSIS_CONTROL_TIME_MS);
    }
}

void OLED_chassis(void)
{
    // OLED_printf(4,4,"YAW_ECD:%d",get_chassis_motor_measure_point(YAW_DOWN)->ecd);
    OLED_printf(0,0,"RH0:%d RH1:%d RH2:%d RH3:%d",rc_p->rc.ch[0],rc_p->rc.ch[1],rc_p->rc.ch[2],rc_p->rc.ch[3]);
}
 